#include "Collision.h"


Collision::Collision(void)
{
}


Collision::~Collision(void)
{
}

void Collision::ShiftBall(Ball &b, vector3 q)
{
	// Move the circle so it rests on top of whatever it collides with
	vector3 shift = (b.transform.position - q).normalized();
	shift = shift * (b.radius);
	b.transform.position = q + shift;
}

void Collision::BounceHinge(Ball &b, HingeBar h, vector3 q)
{
	vector3 momentum = b.transform.momentum;
	if (!b.bounced && momentum.length())
	{
		momentum = quaternion::rotate(momentum, h.transform.orientation.conjugate());
		momentum.y *= -1.0;

		vector3 angleToLinear = h.GetMomentum(q);
		if (momentum.y * angleToLinear.y > 0)
			momentum.y += angleToLinear.y * 0.05;

		momentum = quaternion::rotate(momentum, h.transform.orientation);

		if (momentum.length() > 50)
			b.transform.momentum = momentum;
		else
			b.transform.momentum = vector3();

		b.bounced = true;
	}
	else
	{
		b.bounced = false;
	}
}

void Collision::BounceBar(Ball &b, Bar r, vector3 q)
{
	vector3 momentum = b.transform.momentum;
	if (!b.bounced && momentum.length())
	{
		momentum = quaternion::rotate(momentum, r.transform.orientation.conjugate());
		momentum.y *= -1.0;

		vector3 angleToLinear = r.GetMomentum(q);
		if (momentum.y * angleToLinear.y > 0)
			momentum.y += angleToLinear.y;

		momentum = quaternion::rotate(momentum, r.transform.orientation);

		if (momentum.length() > 50)
			b.transform.momentum = momentum;
		else
			b.transform.momentum = vector3();

		b.bounced = true;
	}
	else
	{
		b.bounced = false;
	}
}

// Gets the closest point of a hinged OBB to a point in space
void Collision::ClosestPointToHinge(vector3 p, HingeBar h, vector3 &q)
{
	// Get the vector from the hinge point of the bar to the point
	vector3 toPoint = p - h.transform.position;

	// Calculate the halfwidth and halflengths
	double hw = h.width/2;
	double hl = h.length/2;

	// Transfer it into the bar's local coordinates
	toPoint = quaternion::rotate(toPoint, h.transform.orientation.conjugate());
	toPoint.x -= hl;

	// Test X axis
	if (toPoint.x > hl)
		toPoint.x = hl;
	else if (toPoint.x < -hl)
		toPoint.x = -hl;

	// Test Y axis
	if (toPoint.y > hw)
		toPoint.y = hw;
	if (toPoint.y < -hw)
		toPoint.y = -hw;

	// Move the resultant vector back into world space
	toPoint.x += hl;
	q = quaternion::rotate(toPoint, h.transform.orientation);
	q += h.transform.position;
}

// Separating axis test for hinged OBBs and Sphere
int Collision::TestBallHinge(Ball b, HingeBar h, vector3 &q)
{
	// Get the closest point from the center of the ball to the bar
	ClosestPointToHinge(b.transform.position, h, q);
	
	// If the distance to the center is less than the radius, return true
	vector3 toBallCenter = q - b.transform.position;
	return toBallCenter.length() <= b.radius;
}

// Gets the closest point of an OBB to a point in space
void Collision::ClosestPointToBar(vector3 p, Bar r, vector3 &q)
{
	// Get the vector from the hinge point of the bar to the point
	vector3 toPoint = p - r.transform.position;

	// Calculate the halfwidth and halflengths
	double hw = r.width/2;
	double hl = r.length/2;

	// Transfer it into the bar's local coordinates
	toPoint = quaternion::rotate(toPoint, r.transform.orientation.conjugate());

	// Test X axis
	if (toPoint.x > hl)
		toPoint.x = hl;
	else if (toPoint.x < -hl)
		toPoint.x = -hl;

	// Test Y axis
	if (toPoint.y > hw)
		toPoint.y = hw;
	if (toPoint.y < -hw)
		toPoint.y = -hw;

	// Move the resultant vector back into world space
	q = quaternion::rotate(toPoint, r.transform.orientation);
	q += r.transform.position;
}

// Separating axis test for OBBs and Sphere
int Collision::TestBallBar(Ball b, Bar r, vector3 &q)
{
	// Get the closest point from the center of the ball to the bar
	ClosestPointToBar(b.transform.position, r, q);
	
	// If the distance to the center is less than the radius, return true
	vector3 toBallCenter = q - b.transform.position;
	return toBallCenter.length() <= b.radius;
}